function [betaMFF, q0] = quatdiff_mult(qHatMF, qMF, firstOrdApprox, smartWrap) %#codegen
%QUATDIFF_MULT 计算四元数的角度差值（乘式差值）
%
% Tips
% # 若采用一阶近似算法，将smartWrap设置为true以避免四元数反向导致误差计算错误
%
% Input Arguments
% # qHatMF: m*4矩阵，比较单位四元数
% # qMF: m*4矩阵，参考单位四元数
% # firstOrdApprox: 逻辑标量，true表示使用一阶近似算法，否则使用精确算法，默认为true
% # smartWrap: 逻辑标量，true表示自动判断使误差最小的四元数正负，否则直接使用输入值计算，默认为true
%
% Output Arguments
% # betaMFF: m*3矩阵，比较值相对于参考值的姿态差值，设qHatMF=qMFHat，则beta_MF^F=alpha_FM^F=theta_FFHat^F为FHat相对于F的旋转矢量（在F/FHat系下的投影），单位为rad
% # q0: m*1向量，-2dqMF°(qMF*)的标量部分，仅在一阶近似算法下输出
%
% Assumptions and Limitations
% # 一阶近似算法在姿态差较大时精度较低，仅供参考
%
% References:
% # 《应用导航算法工程基础》特征四元数的失准角误差

if nargin < 3
    firstOrdApprox = true;
end
if nargin < 4
    smartWrap = true;
end

q0 = NaN(size(qMF, 1), 1);
if firstOrdApprox
    if smartWrap
        dqMF = quatdiff_add(qHatMF, qMF);
    else
        dqMF = qHatMF - qMF;
    end
    tmp = -2*quatmultiply(dqMF, quatconj(qMF));
    betaMFF = tmp(:, 2:4);
    q0 = tmp(:, 1);
else
    qFFHat = quatmultiply(qHatMF, quatconj(qMF));
    betaMFF = -quat2rv(qFFHat);
end
end